function [ flowX flowY ] = HornSchunkRobust(imageSeq, params)
    params.numScales = 1 + floor(log(min(size(imageSeq{1}, 1), size(imageSeq{1}, 2)) / 16) / log(params.pyramidScaleFactor));

    for iIm = 1 : numel(imageSeq) - 1        
        im1F = im2double(MyRGB2Gray(imageSeq{iIm}));
        im2F = im2double(MyRGB2Gray(imageSeq{iIm + 1}));         
                 
         pyramid1 = ImagePyramid(im1F, params);
         pyramid2 = ImagePyramid(im2F, params);
         
         u = zeros(size(pyramid1{params.numScales}));
         v = u;
         for iScale = params.numScales : -1 : 1             
             im1 = pyramid1{iScale};
             im2 = pyramid2{iScale};
             [ x y ] = meshgrid(1 : size(im1, 2), 1 : size(im2, 1));                         

             x2 = x + u;
             y2 = y + v;

             im2Warped = interp2(x, y, im2, x2, y2, 'cubic');        

             du = zeros(size(pyramid1{iScale}));
             dv = du;

             for iIter = 1 : params.numNonLinIterations
                [ lhs rhs ] = Equations(im1, im2Warped, u, v, du, dv, params);                                    
                dFlow = lhs \ rhs;
                du = reshape(dFlow(1 : numel(dFlow) / 2), size(im2));
                dv = reshape(dFlow(1 + numel(dFlow) / 2 : end), size(im2));
             end

             u = u + du;
             v = v + dv;            

             if (params.medianFilterSize > 0)
                 u = medfilt2(u, params.medianFilterSize);
                 v = medfilt2(v, params.medianFilterSize);                       
             end             

             
             if (iScale > 1)
                 u = imresize(u, size(pyramid1{iScale - 1}));
                 v = imresize(v, size(pyramid1{iScale - 1}));           
             end
             
             if params.isVerbose
                fprintf(1, 'Horn Schunk: Image %d/%d | Scale %d/%d Done.\n', iIm, ...
                    numel(imageSeq) - 1, params.numScales - iScale + 1, params.numScales);
             end
         end         
        
        if (numel(imageSeq) > 2)        
            flowX{iIm} = u;
            flowY{iIm} = v;
        else
            flowX = u;
            flowY = v;
        end
        
        if params.isVerbose
            fprintf(1, 'Horn Schunk: Image %d/%d Done.\n', iIm, numel(imageSeq) - 1);
        end
    end
end

function pyramid = ImagePyramid(image, params)    
    for i = 1 : params.numScales
        sigma = i * ( params.pyramidScaleFactor - 1);
        windowSize = max(2 * ceil(norminv(0.9 , 0, sigma)), 10);
        h = fspecial('gaussian', windowSize, sigma);    
        curImage = imfilter(image, h, 'symmetric');
        curImage = imresize(curImage, (1 / params.pyramidScaleFactor)^(i - 1));
        pyramid{i} = curImage;
    end
end

function y = RobustPenalizer(x, request, myEps)    
    if request == 0 % Value
        y = sqrt(x + myEps);
    elseif request == 1 % 1st derivative        
        y = 1 ./ sqrt(x + myEps);
    elseif request == 2 % 2nd derivative
        throw;
    else
        throw;
    end
end

function [ lhs rhs ] = Equations(I1, I2, u, v, du, dv, params)
    [ Ix Iy It  Ixx Iyy Ixy Ixt Iyt, ux, uy, vx, vy  ] = HSGradients(I1, I2, u + du, v + dv, params);    
    
    brightnessPenalty = RobustPenalizer((It + Ix .* du + Iy .* dv) .^2, 1, 1e-4);    
    gradPenalty =   RobustPenalizer((Ixt + Ixx .* du + Ixy .* dv).^2 ...
                                  + (Iyt + Ixy .* du + Iyy .* dv).^2, 1, 1e-2);
                              
    lhsData = BrightnessDataMatrix(Ix, Iy, brightnessPenalty) ...
            + params.gamma * GradDataMatrix(Ixx, Iyy, Ixy, gradPenalty)  ...
            + ConstDataMatrix(Ix);            
    
    smoothPenalty = RobustPenalizer(ux.^2 + uy.^2 + vx.^2 + vy.^2, 1, 1e-6);   
    
    smoothingMat = SmoothnessMatrix(smoothPenalty);
    lhsSmooth = [ smoothingMat, sparse(numel(I1), numel(I1)); sparse(numel(I1), numel(I1)), smoothingMat ];    
    
    lhs = lhsData - params.alpha * lhsSmooth;

    rhs = params.alpha * lhsSmooth * [ u(:) ; v(:) ] ...
       - [ brightnessPenalty(:) .* Ix(:).*It(:); brightnessPenalty(:) .* Iy(:).*It(:) ] ...
       - params.gamma * [ gradPenalty(:) .* (Ixx(:).*Ixt(:) + Ixy(:).*Iyt(:)); ...
                          gradPenalty(:) .* (Ixy(:).*Ixt(:) + Iyy(:).*Iyt(:)) ];
       
end

function dataTerm = ConstDataMatrix(Ix)
    n = numel(Ix);
    constTerm = ones(2 * numel(Ix), 1) * 1e-2;
    dataTerm   = spdiags(constTerm, 0, 2 * n, 2 * n);
end

function dataTerm = GradDataMatrix(Ixx, Iyy, Ixy, penalty)    
    n = numel(Ixx);
    sIx2   = spdiags(penalty(:) .* (Ixx(:).^2 + Ixy(:).^2), 0, n, n);
    sIy2   = spdiags(penalty(:) .* (Iyy(:).^2 + Ixy(:).^2), 0, n, n);
    sIxy   = spdiags(penalty(:) .* (Ixx(:).*Ixy(:) + Ixy(:).*Iyy(:)), 0, n, n);
    dataTerm = [ sIx2, sIxy; sIxy, sIy2 ];   
end

function dataTerm = BrightnessDataMatrix(Ix, Iy, penalty)    
    n = numel(Ix);    
    sIx2   = spdiags(penalty(:) .* Ix(:).^2, 0, n, n);
    sIy2   = spdiags(penalty(:) .* Iy(:).^2, 0, n, n);
    sIxy   = spdiags(penalty(:) .* Ix(:).*Iy(:), 0, n, n);
    dataTerm = [ sIx2, sIxy; sIxy, sIy2 ];   
end